#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Encoder_Interface.h"
#include "TIM.h"
#include "PWM.h"
#include "Uart.h"
#include "DMA_UART1.h"
#include <stdio.h>
#include "Rp.h"
#include "Key.h"
#include "I2C.h"
#include "Mpu6050.h"


static char buf[20];

float target_speed ;
float current_rps;
uint16_t output_duty ;

uint16_t current_encoder_count ;
float Kp,Ki,Kd;

int main(void)
{
    
    RCC_HSEConfig(RCC_HSE_ON);
    
    while(RCC_WaitForHSEStartUp()==ERROR);
    
    RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);
    
    RCC_PLLCmd(ENABLE);
    
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
    
 
    
	/*模块初始化*/
    

	
    //Key_Init();

    
    init_uart_with_dma();
    
    init_dma_for_uart1();
    

    I2C1_Init();
    
    uint8_t * str  = getBuffer();

    
    int length = sprintf((char *)str,"i2c init success \r\n");  
    
   
    send(length);
    
    init_mpu6050();
    
    //OLED_Init();//OLED初始化
    
    str  = getBuffer();
     
    length = sprintf((char *)str,"mpu6050 init success \r\n");  
    
   
    send(length);
    
    
    init_tim();
    
    

    //init_encoder_interface_in_tim3();
    
    //init_my_pwm_in_tim2();
    
    //start_pwm();
    
    //initRp();
    
   
	while (1)
	{
        
             
        static  float accel_buf[3];
        
        static int16_t  gyro_buf[3];
       
        char * errir_tips = read_accel_gyro(accel_buf,gyro_buf);
    
        
        uint8_t * str  = getBuffer();

      
        if(errir_tips){
        
            length = sprintf((char *)str,"%s\r\n",errir_tips);
        }else{
    
        length = sprintf((char *)str,"%f,%f,%f,%d,%d,%d\r\n",
            
              accel_buf[0],
              accel_buf[1],
              accel_buf[2],
              gyro_buf[0],
              gyro_buf[1],
              gyro_buf[2]
          
        );  
        
        }
        
        send(length);
        
	}
}


const float dt = 0.04;

void TIM4_IRQHandler(void){
    
    if(TIM_GetITStatus(TIM4,TIM_IT_Update)){
        
 
        
        TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
    }
}

